#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
//#include "sensor_msgs/Imu.h"
//#include "px4_control_cfg.h"
#include "geometry_msgs/Twist.h" 
//#include "nlink_parser/LinktrackAnchorframe0.h"
//#include "nlink_parser/LinktrackTagframe0.h"
#include "geometry_msgs/PoseStamped.h"
//#include "tello_driver/TelloStatus.h"
#include "tello_show/Position.h"
#include <math.h>
#include <iostream>
#define PI 3.1415926
//#include <Eigen/Dense>
//#include <Eigen/Core>
using namespace std;
double t=0,tstart=0;//定义时间
int i,j,flag = 0,count1=0;//定义标记值
int K = -1;//PID比例增益
double u1[2],u2[2],u3[2],u4[2],u5[2], u6[2];//定义输入量
double tello_pos1[3],tello_pos2[3],tello_pos3[3],tello_pos4[3],tello_pos5[3], tello_pos6[3];//tello位置
double p1[2],p2[2],p3[2],p4[2],p5[2],p6[2];
//定义控制的话题
geometry_msgs::Twist cmd_vel1;
geometry_msgs::Twist cmd_vel2;

int command;//定义控制开关量

//void cb_readPos(const nlink_parser::LinktrackAnchorframe0::ConstPtr& msg)
//用于从动捕/uwb处接受tello的位置信息
void cb_readPos_1(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	geometry_msgs::PoseStamped pos;
	pos = *msg;

	tello_pos1[0] = pos.pose.position.x;
	tello_pos1[1] = pos.pose.position.y;
	tello_pos1[2] = pos.pose.position.z;

}

void cb_readPos_2(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	geometry_msgs::PoseStamped pos;
	pos = *msg;

	tello_pos2[0] = pos.pose.position.x;
	tello_pos2[1] = pos.pose.position.y;
	tello_pos2[2] = pos.pose.position.z;

}

void cb_command(const std_msgs::Int32::ConstPtr& msg)
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;// command = msg.data
	cout << "receive command: " << command << endl;
	if(command == 2 && flag == 0)
	{
		tstart=ros::Time::now().toSec();
		flag=1;
	}
}

void poscmd1(const tello_show::Position::ConstPtr& msg)
{
    tello_show::Position p_cmd1;
	p_cmd1 = *msg;//msg是数据整体，并不是一个指针
	p1[0] = p_cmd1.position_x;
	p1[1] = p_cmd1.position_y;
}
void poscmd2(const tello_show::Position::ConstPtr& msg)
{
    tello_show::Position p_cmd2;
	p_cmd2 = *msg;//msg是数据整体，并不是一个指针
	p2[0] = p_cmd2.position_x;
	p2[1] = p_cmd2.position_y;
}


void vel_limit(double* u)
{
    int i;
    for(i = 0;i < 2;i ++)
    {
        if(K*u[i] > 1)
            u[i] = 1;
        if(K*u[i] < -1)
            u[i] = -1;
    }
}

int main(int argc, char *argv[])
{
    //初始化ros节点
    ros::init(argc,argv,"tello_point");
	ros::NodeHandle nh("");

	ros::Subscriber cmd_sub = nh.subscribe("/command",10,cb_command); 
	//从tello/pose话题中接收无人机的位姿信息
	ros::Subscriber pos_sub1 = nh.subscribe("/tello1/pose", 10, cb_readPos_1);
	ros::Subscriber pos_sub2 = nh.subscribe("/tello2/pose", 10, cb_readPos_2);
    //速度指令
	ros::Publisher vel_pub1 = nh.advertise<geometry_msgs::Twist>("/tello1/cmd_vel",10); 
	ros::Publisher vel_pub2 = nh.advertise<geometry_msgs::Twist>("/tello2/cmd_vel",10); 
    //起飞指令
	ros::Publisher takeoff_pub1 = nh.advertise<std_msgs::Empty>("/tello1/takeoff",10);
	ros::Publisher takeoff_pub2 = nh.advertise<std_msgs::Empty>("/tello2/takeoff",10);
    //降落指令
	ros::Publisher land_pub1 = nh.advertise<std_msgs::Empty>("/tello1/land",10); 
	ros::Publisher land_pub2 = nh.advertise<std_msgs::Empty>("/tello2/land",10); 
	//接受位置指令
	ros::Subscriber pos_cmd1 = nh.subscribe("pos_cmd1", 10, poscmd1);
	ros::Subscriber pos_cmd2 = nh.subscribe("pos_cmd2", 10, poscmd2);
	//判断是否到达
	ros::Publisher pos_arrive1 = nh.advertise<std_msgs::String>("arrive1",10);
	ros::Publisher pos_arrive2 = nh.advertise<std_msgs::String>("arrive2",10);
	std_msgs::String arrive1,arrive2;
	arrive1.data = "arrived",arrive2.data = "arrived";

    command = 0;
	ros::Rate loop_rate(100);

    while(ros::ok())
    {
        if(command == 1)  //起飞
		{	
			std_msgs::Empty takeoff_cmd;
			takeoff_pub1.publish(takeoff_cmd);
			takeoff_pub2.publish(takeoff_cmd);
		}	
        if(command == 2)//进行定点跟踪
        {
            // p1[0] = 0.5,p1[1] = 0.5;
            // p2[0] = 0,p2[1] = 0;
            // p1[0] = 0.5,p1[1] = 0.5;
            // p2[0] = 0,p2[1] = 0;
			u1[0] = (tello_pos1[0] - p1[0]),u1[1] = (tello_pos1[1] - p1[1]);
            u2[0] = (tello_pos2[0] - p2[0]),u2[1] = (tello_pos2[1] - p2[1]);

			//如果无人机抵达了目标位置，则向位置控制节点发送信息
			if(pow(u1[0],2)+pow(u1[1],2) < 0.1)
			{
				pos_arrive1.publish(arrive1);
				ros::Duration(0.5).sleep();
			}
			if(pow(u2[0],2)+pow(u2[1],2) < 0.1)
			{
				pos_arrive2.publish(arrive2);
				ros::Duration(0.5).sleep();
			}

            //限定速度
            vel_limit(u1),vel_limit(u2);

			//确定无人机的速度输入
			cmd_vel1.linear.x=K*u1[0];
			cmd_vel1.linear.y=K*u1[1];
			cmd_vel2.linear.x=K*u2[0];
			cmd_vel2.linear.y=K*u2[1];

			//发布速度指令
            vel_pub1.publish(cmd_vel1);
			vel_pub2.publish(cmd_vel2);	
			if(command == 9)  //降落
			{	
				std_msgs::Empty land_cmd;
				land_pub1.publish(land_cmd);	
				land_pub1.publish(land_cmd);
				cmd_vel1.linear.x=0;
				cmd_vel1.linear.y=0;
				cmd_vel2.linear.x=0;
				cmd_vel2.linear.y=0;
				vel_pub1.publish(cmd_vel1);
				vel_pub2.publish(cmd_vel2);								
			}
			count1 ++;
        }
		//停止水平运动，保持定高飞行（便于降落）,注意一定要输入一次命令3，否则控制指令会一直发
		if(command == 3)
		{
			cmd_vel1.linear.x=0;
			cmd_vel1.linear.y=0;
			cmd_vel2.linear.x=0;
			cmd_vel2.linear.y=0;
			vel_pub1.publish(cmd_vel1);
			vel_pub2.publish(cmd_vel2);
			if(command == 9)  //降落
			{	
				std_msgs::Empty land_cmd;
				land_pub1.publish(land_cmd);	
				land_pub1.publish(land_cmd);
				cmd_vel1.linear.x=0;
				cmd_vel1.linear.y=0;
				cmd_vel2.linear.x=0;
				cmd_vel2.linear.y=0;
				vel_pub1.publish(cmd_vel1);
				vel_pub2.publish(cmd_vel2);								
			}

		}
		if(command == 9)  //降落
		{	
			std_msgs::Empty land_cmd;
			land_pub1.publish(land_cmd);	
			land_pub1.publish(land_cmd);
			cmd_vel1.linear.x=0;
			cmd_vel1.linear.y=0;
			cmd_vel2.linear.x=0;
			cmd_vel2.linear.y=0;
			vel_pub1.publish(cmd_vel1);
			vel_pub2.publish(cmd_vel2);								
		}	
        ros::spinOnce();
		loop_rate.sleep();
    }


    return 0;
}
